#include "pathplan.h"
#include <cmath>
#include <iostream>

PATHPLAN::PATHPLAN()
{
    scan=NULL;
}

PATHPLAN::~PATHPLAN()
{
    scan=NULL;
}

void PATHPLAN::GetCamerView(QColor *image, int height, int width)
{
    viewHeight=height;
    viewWidth=width;
    scan=image;
}

void PATHPLAN::CalNextPoint(double &x, double &y, double &theata,double speed)
{
    int targetPosX=0;
    int targetPosY=0;
    bool flag=false;
    double deltaTheata=0.0;
    for(int i=viewHeight-1;i>=0;--i)
    {
        for(int j=0;j<viewWidth;++j)
        {
            if(this->scan[i*(viewWidth)+j].black()>30)
            {
                targetPosX=j;
                flag=true;
                break;
            }
        }
        if(flag)
        {
            targetPosY=i+1;
            break;
        }
    }
    deltaTheata=atan((targetPosX-viewWidth/2.0)*1.0/targetPosY);
    theata+=(deltaTheata);
    x+=speed*sin(theata);
    y+=speed*cos(theata);
}
